from __future__ import print_function
import time
import re
import serial
import numpy as np

global count_get_uwb  # 只执行一次
global plot_x
global plot_y  # 画图用全局变量坐标
global x
global y

plot_x=[]
plot_y=[]
x=0
y=0
count_get_uwb=0

def plot_pos(A,B):  # 绘制飞行轨迹
    global plot_x
    global plot_y
    plot_x.append(A)
    plot_y.append(B)
    print("max and min of [x] value",max(plot_x),min(plot_x))
    print("max and min of [y] value",max(plot_y),min(plot_y))

# 绘图

def get_uwb():
    global plot_x
    global plot_y
    global count_get_uwb
    global x
    global y
    correct=1
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)  # change tty/baud rate/timeout here
    while True:
        line = ser.readline()  # read position info
        line = str(line)  # byte->str
        if line:  # if accepted
            a = re.search('KT', line)  # find xyz information
            b = re.search('NULL', line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b:
                    temp_x=x
                    temp_y=y# last round data
                    #                 print(type(line))
                    count = line.split(',', 7)  # standard
                    #                     print ("count=",count)
                    temp1 = count[5].split('(')
                    temp2 = count[7].split(')')
                    position = [0, 0, 0]
                    position[0] = temp1[1]
                    position[1] = count[6]
                    position[2] = temp2[0]
                    x = float(position[1])  # 本轮收到的位置信息
                    y = float(position[0])
                    z = float(position[2])
                    print("Original UWB information:",x,y,z)
                    if count_get_uwb == 0:
                        temp_x = x
                        temp_y = y
                        count_get_uwb = 1
                    s1 = x - temp_x  # 计算两次差值
                    s2 = y - temp_y
                    if(abs(s1)>correct):#如果差值大于0.2进行修正
                        x=temp_x+np.sign(s1)*correct
                        print("Value [X] is corrected")
                    if(abs(s2)>correct):
                        y=temp_y+np.sign(s2)*correct
                        print("Value [Y] is corrected")
                    print("Camera position [Processed]: ", x, y, z)
#                     plot_pos(y,x)
                    break
        else:
            break
    return
    


# plt.ion()
while True:
    get_uwb()
    time.sleep(1)
    
    
    
print("This is the end of program")
  
